% Closing al figures before start
close all;

% Define Start and End positions
T1 = SE3(0,0,0) * SE3.Rx(pi);
% Define a second position  in the same way


%% Create Joint Motion
% Calculate the Joints angles of the trajectories
J1 = panda.ikine(T1,'q0',XYZ);%replace XYZ
J2 = panda.ikine(T2,'q0',XYZ);%replace XYZ

% Use jtraj to create the trajectory between start and end values of the
% joint angles
qj = jtraj(startJoint,endJoint,steps);%enter the correct number of steps, also enter the correct start and end joint angle value

% Use the forward kinematics to create the time series of the
% trajectory
Tj = panda.fkine(jointtrajectory); % enter the correct joint trajectory time series

% Prefill a variable with the right dimensions (3 coorindates and 50 time
% steps
xyzj = zeros(50,3);

% Extract the translation values and store them in our xyzj variable
for index=1:1:50
    xyzj(index,1) = Tj(index).t(1,1);
    % repeat this for Y and Z values, be sure to modify the right elements
end

% Plot the X, Y, Z Variable over time
xyzFigure = figure();
hold on;
plot(xyzj);

% Plot the trajectory in the XY Plane
xyPlaneFigure = figure();
hold on;
plot(x-values,y-Values) % enter the right variable names for the x and y values
xlim([0.2 0.6]); % Adjust Axis Limits
ylim([-0.2 0.2]);% Adjust Axis Limits

% Plot the Joint Angles
jointAngleFigure = figure();
hold on;
plot(jointAngletimeseries); % select the right value for the joint angle time series

%% Create Cartesian Motion

% Create the carteisan trjectory
Tc = ctraj(startTrafo,EndTrafo,steps); % enter the correct variable names for our start, end position and number of time steps

% create the time series of the joint angles for the cartesian trajectorz
qc = panda.ikine(cartesianetimeseries,'q0',qStart'); % enter the correct variable name for the cartesian trajectory

% Prefill another variable for the cartesian trajectory
xyzc = zeros(50,3);

% Extract the trnaslation values for the cartesian trajectory
for index=1:1:50
    xyzc(index,1) = Tc(index).t(1,1);
    % add Y and Z values

end

% Plot the trajectory in the XY Plane
figure(xyPlaneFigure);
plot(x-values,y-values); % use correct variable names for x and y values

% Plot the X, Y, Z Variable over time
figure(xyzFigure);
plot(xyzc);

% Plot the logged data
% After you imported the file from the CPP App uncomment the next line
%plot(log.x,log.y",LineWidth",2)


